import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np

class ImagePublisher(Node):
    def __init__(self):
        super().__init__('jpeg_publisher')
        self.publisher_ = self.create_publisher(CompressedImage, '/camera/image/compressed', 10)
        timer_period = 0.1  # 10Hz 发布频率
        self.timer = self.create_timer(timer_period, self.publish_image)
        self.cap = cv2.VideoCapture(0)  # 0 表示默认摄像头
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

    def publish_image(self):
        ret, frame = self.cap.read()
        if ret:
            # 转换为 JPEG 格式
            _, jpeg_buf = cv2.imencode('.jpg', frame)

            # 填充 ROS 消息
            msg = CompressedImage()
            msg.header.stamp = self.get_clock().now().to_msg()
            msg.format = "jpeg"
            msg.data = np.array(jpeg_buf).tobytes()

            self.publisher_.publish(msg)
            self.get_logger().info('Publishing JPEG image (Size: %d bytes)' % len(msg.data))

def main(args=None):
    rclpy.init(args=args)
    node = ImagePublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()